#ifndef __ROB_SERVO_H__
#define __ROB_SERVO_H__

#include "rob_common.h"
#include "mfl.h"  /* Include EASML Math Float Library for PID control */

/* Servo Configuration Structure */
typedef struct {
    uint16 PwmMin_u16;     /* Minimum PWM value (us) */
    uint16 PwmMax_u16;     /* Maximum PWM value (us) */
    uint16 PwmCenter_u16;  /* Center PWM value (us) */
    float32 AngleMin_f32;  /* Minimum angle (degrees) */
    float32 AngleMax_f32;  /* Maximum angle (degrees) */
    sint16 Offset_s16;     /* PWM offset calibration */
    boolean Reverse_b;     /* Reverse direction flag */
} Rob_ServoConfig_Type;

/* Servo State Structure */
typedef struct {
    float32 CurrentAngle_f32;  /* Current angle (degrees) */
    float32 TargetAngle_f32;   /* Target angle (degrees) */
    uint16 CurrentPwm_u16;     /* Current PWM value (us) */
    uint16 TargetPwm_u16;      /* Target PWM value (us) */
    float32 Velocity_f32;      /* Angular velocity (deg/s) */
    uint16 MoveTime_u16;       /* Movement time (ms) */
    uint16 ElapsedTime_u16;    /* Elapsed time (ms) */
    boolean IsMoving_b;        /* Movement status */
    Rob_Status_Type Status;    /* Servo status */
} Rob_ServoState_Type;

/* Servo PID Control Structure (using EASML PID) */
typedef struct {
    Mfl_StatePID_Type PidState;   /* EASML PID state */
    Mfl_ParamPID_Type PidParam;   /* EASML PID parameters */
    Mfl_Limits_Type PidLimits;    /* PID output limits */
    float32 SampleTime_f32;       /* Control sample time (ms) */
    boolean Enable_b;             /* PID enable flag */
} Rob_ServoPID_Type;

/* Servo System Structure */
typedef struct {
    Rob_ServoConfig_Type Config[ROB_SERVO_NUM];  /* Servo configurations */
    Rob_ServoState_Type State[ROB_SERVO_NUM];    /* Servo states */
    Rob_ServoPID_Type Pid[ROB_SERVO_NUM];        /* PID controllers */
    uint32 LastUpdate_u32;                       /* Last update timestamp */
    Rob_Status_Type SystemStatus;                /* Overall system status */
} Rob_ServoSystem_Type;

/* Trajectory Point Structure */
typedef struct {
    float32 Angle_f32;    /* Target angle */
    uint16 Time_u16;      /* Time to reach target (ms) */
    uint8 Interpolation;  /* Interpolation type */
} Rob_TrajectoryPoint_Type;

/* Movement Profile Structure */
typedef struct {
    Rob_TrajectoryPoint_Type Points[ROB_MAX_TRAJECTORY_POINTS];
    uint16 PointCount_u16;
    uint16 CurrentPoint_u16;
    boolean IsActive_b;
} Rob_MovementProfile_Type;

/* Function Declarations */

/* Initialization Functions */
Rob_Status_Type Rob_ServoInit(Rob_ServoSystem_Type *System_pst);
Rob_Status_Type Rob_ServoConfigInit(Rob_ServoConfig_Type *Config_pst, 
                                   uint8 ServoIndex_u8);

/* Basic Servo Control */
Rob_Status_Type Rob_ServoSetAngle(Rob_ServoSystem_Type *System_pst,
                                 uint8 ServoIndex_u8, float32 Angle_f32);

Rob_Status_Type Rob_ServoSetPwm(Rob_ServoSystem_Type *System_pst,
                               uint8 ServoIndex_u8, uint16 PwmValue_u16);

Rob_Status_Type Rob_ServoMove(Rob_ServoSystem_Type *System_pst,
                             uint8 ServoIndex_u8, float32 TargetAngle_f32,
                             uint16 MoveTime_u16);

/* Advanced Control Functions */
Rob_Status_Type Rob_ServoMoveAll(Rob_ServoSystem_Type *System_pst,
                                const float32 Angles_pf32[ROB_SERVO_NUM],
                                uint16 MoveTime_u16);

Rob_Status_Type Rob_ServoInterpolateMove(Rob_ServoSystem_Type *System_pst,
                                        uint8 ServoIndex_u8,
                                        float32 StartAngle_f32,
                                        float32 EndAngle_f32,
                                        uint16 Duration_u16);

/* PID Control Functions (using EASML) */
Rob_Status_Type Rob_ServoPIDInit(Rob_ServoPID_Type *Pid_pst,
                                float32 Kp_f32, float32 Ki_f32, float32 Kd_f32);

Rob_Status_Type Rob_ServoPIDUpdate(Rob_ServoPID_Type *Pid_pst,
                                  float32 Setpoint_f32, float32 Feedback_f32,
                                  float32 *Output_pf32, float32 DeltaTime_f32);

/* Conversion Functions */
uint16 Rob_ServoAngleToPwm_u16(const Rob_ServoConfig_Type *Config_cpst,
                              float32 Angle_f32);

float32 Rob_ServoPwmToAngle_f32(const Rob_ServoConfig_Type *Config_cpst,
                               uint16 PwmValue_u16);

/* Calibration Functions */
Rob_Status_Type Rob_ServoCalibrate(Rob_ServoSystem_Type *System_pst,
                                  uint8 ServoIndex_u8, sint16 Offset_s16);

Rob_Status_Type Rob_ServoSetReverse(Rob_ServoSystem_Type *System_pst,
                                   uint8 ServoIndex_u8, boolean Reverse_b);

/* Status and Monitoring */
Rob_Status_Type Rob_ServoGetStatus(const Rob_ServoSystem_Type *System_cpst,
                                  uint8 ServoIndex_u8);

boolean Rob_ServoIsMoving_b(const Rob_ServoSystem_Type *System_cpst,
                           uint8 ServoIndex_u8);

boolean Rob_ServoAllStopped_b(const Rob_ServoSystem_Type *System_cpst);

/* System Update Functions */
Rob_Status_Type Rob_ServoUpdate(Rob_ServoSystem_Type *System_pst,
                               uint32 CurrentTime_u32);

Rob_Status_Type Rob_ServoEmergencyStop(Rob_ServoSystem_Type *System_pst);

/* Trajectory Functions */
Rob_Status_Type Rob_ServoTrajectoryInit(Rob_MovementProfile_Type *Profile_pst);

Rob_Status_Type Rob_ServoTrajectoryAddPoint(Rob_MovementProfile_Type *Profile_pst,
                                           float32 Angle_f32, uint16 Time_u16);

Rob_Status_Type Rob_ServoTrajectoryExecute(Rob_ServoSystem_Type *System_pst,
                                          uint8 ServoIndex_u8,
                                          const Rob_MovementProfile_Type *Profile_cpst);

#endif /* __ROB_SERVO_H__ */
